#ifndef RSIM_BALL_H
#define RSIM_BALL_H

/* -------------------------------------------------------------------------- *
 * File: Ball.h                                                               *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Joint.h"
#include "Utils.h"

namespace RSIM{

class RigidBodyNode;
class ABAData;
class EOMData;

class Ball:public Joint{
        private:
                /** Create a ball joint with M and F coincident in default configuration */
                Ball(const int& NodeIndex);
        public:
                /** Create a new joint */
                static Ball* New(const int& NodeIndex);				
                
                /** Print the joint type */
                void printJointType() const;
                
                const bool areQuaternionsInUse()const{return true;}
                
                int getDOF()const{return 3;}
                
                void getHColumn(const int& uindex, 
                                double *hptr)const;
                
                void setNQ(int *q)const{q[0]=4;}
                
                void setNU(int *u)const{u[0]=3;}
                
                /** fills *q with default values of q. */
                void setDefaultQ(double *q)const;
                
                /** fills *u with default values of q*/
                void setDefaultU(double *u)const;                                
                
                void calcShiftPartialLinear(const Vect3& beta,
                                            const int& uindex, 
                                            const Rotation& R_NF,
                                            double *dv)const;
                                
                /** Update joint Transform and calculate betak*/
                void updJointTransform(const double *q,
                                const RigidBodyNode *Parent,
                                const int& pChildID,
                                double *betak);
                                
                void updVelocity(const State& state, 
                                const RigidBodyNode *Parent, 
                                const Rotation& prF_C_F, 
                                const double *betak,
                                Vect3& w,
                                Vect3& v) const ;
                
                void updRelVelocity(const State& state, Vect3& w, Vect3& v) const;
                
                int getJointDOF() const;
                
                void calcAKT(   const RigidBodyNode *Parent, 
                                const RigidBodyNode *Body,
                                const int& ChildID,
                                const double *betak,
                                State& state,
                                Vect6& sAkt) const;
                
                void calcQDot(State& state)const;
                
                void calc_H_x_UDot(const State& state, double *h_x_udot) const;
                
                void sweepForward(const RigidBodyNode *Parent,
                                const int& ChildID,
                                const Vect6& sAkt,
                                const Rotation& FCF,
                                const ABAData *aba,
                                const double *betak,
                                EOMData *eom,
                                State& state
                                );
                                
                void sweepBackwards(const bool& isLeaf, 
                                const int& ChildID,
                                const Vect6& sAkt,
                                const Rotation& FCF,
                                const EOMData *eom,
                                const double *betak,
                                RigidBodyNode *Parent,
                                ABAData *aba) const;

}; // class Ball

//////////////////////////////////////////////////////

inline void Ball::calcShiftPartialLinear(const Vect3& beta,
                                         const int& uindex, 
                                         const Rotation& R_NF,
                                         double *dv)const{
        const double *r_nf = R_NF.Ptr();
        
        // C stores R_NF*partial(ang)
        double C[3];
        C[0] = r_nf[uindex];C[1] = r_nf[3+uindex];C[2] = r_nf[6+uindex];
        
        // beta x (R_NF*patrial(ang))
        FastOps::cross(C,beta.Ptr(),dv);        
}

//////////////////////////////////////////////////////

inline void Ball::getHColumn(const int& uindex, 
                        double *hptr)const{
        #ifdef RSIM_SAFE
        Utils::rsim_assert(uindex<3,"Joint::Ball::getHColumn(), uindex must be less than 3");
        Utils::rsim_assert(uindex>=0,"Joint::Ball::getHColumn(), uindex must be great than or equal to 0");
        #endif
        
        hptr[0] = 0.0;  hptr[1] = 0.0;  hptr[2] = 0.0;
        hptr[3] = 0.0;  hptr[4] = 0.0;  hptr[5] = 0.0;
        hptr[uindex]=1.0;
}

}// namespace RSIM 

#endif